/*
** Copyright (C) 2010 Calin Radoni
**
** This file is part of CHScanner.
**
** CHScanner is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** CHScanner is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with CHScanner. If not, see <http://www.gnu.org/licenses/>.
*/

#include "StdAfx.h"
#include ".\packetbuilder.h"

#include <Winsock2.h>

//---------------------------------------------------------------------------

PacketBuilder::PacketBuilder(void)
{
int i;
	for(i=0; i<6; i++){
		ethFF[i] = 0xFF;
		eth00[i] = 0x00;
	}

	packet   = NULL;
	packSize = 0;
}

PacketBuilder::~PacketBuilder(void)
{
	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}
	packSize = 0;
}

//---------------------------------------------------------------------------
USHORT PacketBuilder::ComputeIpCheckSum(USHORT* buh, int len)
{
unsigned long sum;
unsigned short oddbyte;
unsigned short answer;

	sum=0;
	while(len>1){
		sum += *buh++;
		len -= 2;
	}
	if(len==1){
		oddbyte=0;
		*((unsigned char*)&oddbyte) = *(unsigned char*)buh;
		sum += oddbyte;
	}

	sum = (sum>>16) + (sum & 0xFFFF);
	sum+= (sum>>16);
	answer =(unsigned short) ~sum;
	return answer;
}

USHORT PacketBuilder::ComputeCheckSum(USHORT *buffer, int size, USHORT* prefix)
{
unsigned long cksum;
int i;
	cksum=0;

	if(prefix!=NULL){
		for(i=0;i<6;i++)
			cksum+=prefix[i];
	}

	while(size>1){
		cksum+=*buffer++;
		size-=sizeof(USHORT);
	}

	if(size){
		cksum += *(UCHAR*)buffer;
	}

	cksum = (cksum >> 16) + (cksum & 0xffff);
	cksum += (cksum >>16);
	return (USHORT)(~cksum);
}

//---------------------------------------------------------------------------
bool PacketBuilder::BuildIPHeader(BYTE* destIn, DWORD dwSA, DWORD dwDA, BYTE bTOS, WORD wLen, WORD wID, WORD wFragOff, BYTE bTTL, BYTE bProto)
{
	return BuildIPHeaderWithOptions(destIn, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, bProto, NULL, 0);
}

bool PacketBuilder::BuildIPHeaderWithOptions(
		BYTE* destIn, DWORD dwSA, DWORD dwDA, BYTE bTOS, WORD wLen, WORD wID, WORD wFragOff, BYTE bTTL, BYTE bProto, BYTE* bpIpOpt, WORD wIpOptLen)
{
IPHeaderPG* iph;
unsigned short ccc;
DWORD ula;
WORD xLV;

	if(destIn==NULL) return false;
	if(bpIpOpt==NULL && wIpOptLen!=0) return false;

	if(wIpOptLen!=0){
		xLV = sizeof(IPHeaderPG) + wIpOptLen;
		xLV = xLV >> 2;
		if(xLV<5) return false;
		if(xLV>15) return false;
		xLV |= 0x40;
	}
	else xLV = 0x45;
	
	iph = (IPHeaderPG*)destIn;

	iph->HLV        = (BYTE)xLV;
	iph->Tos        = bTOS;
	iph->Length     = htons(wLen);
	iph->Id         = htons(wID);
	iph->FragOffset = htons(wFragOff);
	iph->TTL        = bTTL;
	iph->Protocol   = bProto;
	iph->ChkSum     = 0x0000;

	ula = dwSA;
		iph->Source[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Source[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Source[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Source[3] = (BYTE)(ula&0x000000FF);
	ula = dwDA;
		iph->Destination[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Destination[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Destination[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		iph->Destination[3] = (BYTE)(ula&0x000000FF);

BYTE* bbb;
	bbb = (BYTE*)&(destIn[sizeof(IPHeaderPG)]);
	memcpy(bbb, bpIpOpt, wIpOptLen);

//	ccc = ComputeIpCheckSum((unsigned short*)iph, sizeof(IPHeaderPG));
	ccc = ComputeIpCheckSum((unsigned short*)destIn, sizeof(IPHeaderPG) + wIpOptLen);

	iph->ChkSum = ccc;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketTCP(
			DWORD dwSA, DWORD dwDA,
			WORD wSP, WORD wDP,

			BYTE bTOS,
			WORD wID,
			WORD wFragOff,
			BYTE bTTL,

			DWORD dwSeq,
			DWORD dwAck,
			BYTE bFlags,
			WORD wWindow,
			WORD wUrgent,

			BYTE* bpOpt,
			DWORD dwOptLen,
			
			BYTE* bpData,
			DWORD dwDataLen
	)
{
DWORD ula;
DWORD ttsize;

	packSize = 0;

	if(bpOpt==NULL && dwOptLen!=0) return false;

	if(dwOptLen&0x00000003) return false;
		// should be an integral number of 32 bit words

	if(bpData==NULL && dwDataLen!=0) return false;

	ttsize  = sizeof(IPHeaderPG);
	ttsize += sizeof(TCPHeaderPG);
	ttsize += dwOptLen;
	ttsize += dwDataLen;

	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}
	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

WORD wLen;
	wLen = (WORD)ttsize;
	if(!BuildIPHeader(packet, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, IPPROTO_TCP)){
		delete[] packet;
		packet = NULL;
		return false;
	}

TCPHeaderPG* tcph;
HeaderPrefixPG tcphpx;

	tcph = (TCPHeaderPG*)&(packet[sizeof(IPHeaderPG)]);

	ula = dwSA;
		tcphpx.Source[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Source[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Source[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Source[3] = (BYTE)(ula&0x000000FF);
	ula = dwDA;
		tcphpx.Destination[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Destination[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Destination[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		tcphpx.Destination[3] = (BYTE)(ula&0x000000FF);


	tcphpx.Zero     = 0;
	tcphpx.Protocol = IPPROTO_TCP;

	wLen = (WORD)(sizeof(TCPHeaderPG) + dwOptLen + dwDataLen);
	tcphpx.LenHi = (BYTE)((wLen>>8)&0x00FF);
	tcphpx.LenLo = (BYTE)(wLen&0x00FF);

	tcph->SourcePort      = htons(wSP);
	tcph->DestinationPort = htons(wDP);

	tcph->Seq = dwSeq;
	tcph->Ack = dwAck;

	wLen = (WORD)(sizeof(TCPHeaderPG) + dwOptLen);
	wLen = (WORD)(wLen>>2);
	wLen = (WORD)(wLen<<4);
	wLen = (WORD)(wLen&0x00FF);
	tcph->Offset = (BYTE)wLen;

	tcph->Flags = bFlags;

	tcph->Window = htons(wWindow);

	tcph->Checksum = 0x0000;

	tcph->UrgentPointer = wUrgent;

BYTE* bbb;
	if(bpOpt!=NULL){
		bbb = &(packet[sizeof(IPHeaderPG) + sizeof(TCPHeaderPG)]);
		memcpy(bbb, bpOpt, dwOptLen);
	}

	if(bpData!=NULL){
		bbb = &(packet[sizeof(IPHeaderPG) + sizeof(TCPHeaderPG) + dwOptLen]);
		memcpy(bbb, bpData, dwDataLen);
	}

	tcph->Checksum = ComputeCheckSum(
					(unsigned short*)tcph,
					sizeof(TCPHeaderPG) + dwOptLen + dwDataLen,
					(unsigned short*)&tcphpx);

	packSize = ttsize;
	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketICMP(
			DWORD dwSA, DWORD dwDA,

			BYTE bTOS,
			WORD wID,
			WORD wFragOff,
			BYTE bTTL,

			BYTE bType,
			BYTE bCode,
			WORD wIDicmp,
			WORD wSeq,

			BYTE* bpData,
			DWORD dwDataLen
	)
{
DWORD ttsize;

	packSize = 0;

	if(bpData==NULL && dwDataLen!=0) return false;

	ttsize  = sizeof(IPHeaderPG);
	ttsize += sizeof(ICMPHeaderPG);
	ttsize += dwDataLen;

	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}
	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

WORD wLen;
	wLen = (WORD)ttsize;
	if(!BuildIPHeader(packet, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, IPPROTO_ICMP)){
		delete[] packet;
		packet = NULL;
		return false;
	}

ICMPHeaderPG* icmph;

	icmph=(ICMPHeaderPG*)&(packet[sizeof(IPHeaderPG)]);

	icmph->Type     = bType;
	icmph->Code     = bCode;
	icmph->Checksum = 0x0000;
	icmph->Id       = wIDicmp;
	icmph->Seq      = wSeq;

BYTE* bbb;

	if(bpData!=NULL){
		bbb=&(packet[sizeof(IPHeaderPG) + sizeof(ICMPHeaderPG)]);
		memcpy(bbb, bpData, dwDataLen);
	}

	icmph->Checksum = ComputeCheckSum(
					(unsigned short*)icmph,
					sizeof(ICMPHeaderPG) + dwDataLen,
					NULL);

	packSize = ttsize;
	return true;
}


//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketEmptyIpProto(
			DWORD dwSA, DWORD dwDA,		// source and destination address

			BYTE bTOS,		// type of service
			WORD wID,		// ip id
			WORD wFragOff,	// fragment offset + fragment flags
			BYTE bTTL,		// time to live

			BYTE bProtocol	// protocol
	)
{
DWORD ttsize;

	packSize = 0;
	ttsize   = sizeof(IPHeaderPG);

	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}
	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

WORD wLen;
	wLen = (WORD)ttsize;
	if(!BuildIPHeader(packet, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, bProtocol)){
		delete[] packet;
		packet = NULL;
		return false;
	}

	packSize = ttsize;
	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketUDP(
			DWORD dwSA, DWORD dwDA,		// source and destination address

			BYTE bTOS,		// type of service
			WORD wID,		// ip id
			WORD wFragOff,	// fragment offset + fragment flags
			BYTE bTTL,		// time to live

			WORD wSrcPort,
			WORD wDstPort,

			BYTE* bpData,	// data
			DWORD dwDataLen	// length of data
		)
{
DWORD ula;
DWORD ttsize;

	packSize = 0;

	if(bpData==NULL && dwDataLen!=0) return false;

	ttsize  = sizeof(IPHeaderPG);
	ttsize += sizeof(UDPHeaderPG);
	ttsize += dwDataLen;

	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}
	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

WORD wLen;
	wLen = (WORD)ttsize;
	if(!BuildIPHeader(packet, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, IPPROTO_UDP)){
		delete[] packet;
		packet = NULL;
		return false;
	}

UDPHeaderPG* udph;
HeaderPrefixPG udphpx;

	udph = (UDPHeaderPG*)&(packet[sizeof(IPHeaderPG)]);

	ula = dwSA;
		udphpx.Source[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Source[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Source[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Source[3] = (BYTE)(ula&0x000000FF);
	ula = dwDA;
		udphpx.Destination[0] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Destination[1] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Destination[2] = (BYTE)(ula&0x000000FF);ula=ula>>8;
		udphpx.Destination[3] = (BYTE)(ula&0x000000FF);


	udphpx.Zero     = 0;
	udphpx.Protocol = IPPROTO_UDP;

	wLen = (WORD)(sizeof(UDPHeaderPG) + dwDataLen);
	udphpx.LenHi = (BYTE)((wLen>>8)&0x00FF);
	udphpx.LenLo = (BYTE)(wLen&0x00FF);

	wLen = (WORD)(sizeof(UDPHeaderPG) + dwDataLen);

	udph->SourcePort      = htons(wSrcPort);
	udph->DestinationPort = htons(wDstPort);
	udph->Length          = htons(wLen);
	udph->Checksum        = 0x0000;

BYTE* bbb;

	if(bpData!=NULL){
		bbb = &(packet[sizeof(IPHeaderPG) + sizeof(UDPHeaderPG)]);
		memcpy(bbb, bpData, dwDataLen);
	}

	udph->Checksum = ComputeCheckSum(
					(unsigned short*)udph,
					sizeof(UDPHeaderPG) + dwDataLen,
					(unsigned short*)&udphpx);

	packSize = ttsize;
	
	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketARP(
			WORD arpCode,
			BYTE srcMacEthIn[6],
			BYTE srcMacIn[6],
			BYTE srcIpIn[4],
			BYTE dstMacEthIn[6],
			BYTE dstMacIn[6],
			BYTE dstIpIn[4]
	    )
{
DWORD ttsize;
DWORD qqsize;

	switch(arpCode){
		case 1 :	// request
			ttsize = 0x2A;
			break;
		case 2 :	// replay
			ttsize = 0x3C;
			break;
		default :
			return false;
	}

	packSize = 0;
	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}

	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

FullARPPacket arpp;

	memcpy(arpp.ethDstMAC, dstMacEthIn, 6);
	memcpy(arpp.ethSrcMAC, srcMacEthIn, 6);
	arpp.ethId    = htons(0x0806);

	arpp.haSpace  = htons(0x0001);
	arpp.paSpace  = htons(0x0800);
	arpp.haLength = 0x06;
	arpp.paLength = 0x04;
	arpp.arpCode  = htons(arpCode);

	memcpy(arpp.senderHA,   srcMacIn, 6);
	memcpy(arpp.senderPA,   srcIpIn,  4);
	memcpy(arpp.receiverHA, dstMacIn, 6);
	memcpy(arpp.receiverPA, dstIpIn,  4);

	memset(packet, 0x00, ttsize);
	qqsize = sizeof(FullARPPacket);
	if(qqsize>ttsize)
		qqsize = ttsize;
	memcpy(packet, &arpp, qqsize);

	packSize = ttsize;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketARPDotQ(
			WORD arpCode,
			BYTE srcMacEthIn[6],
			BYTE srcMacIn[6],
			BYTE srcIpIn[4],
			BYTE dstMacEthIn[6],
			BYTE dstMacIn[6],
			BYTE dstIpIn[4],
			BYTE priorityIn,
			WORD vlanIdIn
	    )
{
DWORD ttsize;
DWORD qqsize;
WORD wVal;

	switch(arpCode){
		case 1 :	// request
			ttsize = 0x2E;
			break;
		case 2 :	// replay
			ttsize = 0x40;
			break;
		default :
			return false;
	}

	packSize = 0;
	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}

	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

FullARPPacketDotQ arpp;

	memcpy(arpp.ethDstMAC, dstMacEthIn, 6);
	memcpy(arpp.ethSrcMAC, srcMacEthIn, 6);

	arpp.tpid = htons(0x8100);			// 802.1Q / 802.1P identifier

	/*
	tci:
		- User priority: priorityIn (Ex.: GVRP = 7, ICMP = 4, Others = 0)
		- CFI: 0
		- VID: vlanIdIn
	*/
	wVal = (WORD)priorityIn;
	wVal = wVal << 13;
	wVal &= 0xE000;
	wVal += vlanIdIn & 0x0FFF;
	arpp.tci = htons(wVal);

	arpp.ethId    = htons(0x0806);		// ARP identifier

	arpp.haSpace  = htons(0x0001);
	arpp.paSpace  = htons(0x0800);
	arpp.haLength = 0x06;
	arpp.paLength = 0x04;
	arpp.arpCode  = htons(arpCode);

	memcpy(arpp.senderHA,   srcMacIn, 6);
	memcpy(arpp.senderPA,   srcIpIn,  4);
	memcpy(arpp.receiverHA, dstMacIn, 6);
	memcpy(arpp.receiverPA, dstIpIn,  4);

	memset(packet, 0x00, ttsize);
	qqsize = sizeof(FullARPPacket);
	if(qqsize>ttsize)
		qqsize = ttsize;
	memcpy(packet, &arpp, qqsize);

	packSize = ttsize;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketIGMPQuery(
	DWORD dwSA, DWORD dwDA,		///< source and destination address

	BYTE bTOS,					///< type of service
	WORD wID,					///< ip id
	WORD wFragOff,				///< fragment offset + fragment flags
	BYTE bTTL,					///< time to live

	BYTE version,				///< 1, 2 or 3
	BYTE maxResponseTime,		///< ignored for version 1 = 0
	BYTE groupAddress[4]		///< ignored for version 1 = 0.0.0.0
)
{
DWORD ttsize;

	packSize = 0;
	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}

	if(version<1 || version>3) return false;

	ttsize  = sizeof(IPHeaderPG);
	if(version!=3) ttsize += 12;	// router option + IGMP packet V1 or V2
	else           ttsize += 16;	// router option + IGMP packet V3

	try{
		packet = new BYTE[ttsize];
	}
	catch(...){
		packet = NULL;
	}
	if(packet==NULL) return false;

BYTE bbb[4];
	bbb[0] = 0x94;
	bbb[1] = 0x04;
	bbb[2] = 0x00;
	bbb[3] = 0x00;

WORD wLen;
	wLen = (WORD)ttsize;
	if(!BuildIPHeaderWithOptions(packet, dwSA, dwDA, bTOS, wLen, wID, wFragOff, bTTL, IPPROTO_IGMP, bbb, 4)){
		delete[] packet;
		packet = NULL;
		return false;
	}

IGMPHeaderPG *igmph;
	igmph = (IGMPHeaderPG*)&(packet[sizeof(IPHeaderPG) + 4]);
	igmph->Type = 0x11;	// Membership Query
	igmph->ChkSum = 0;
	if(version==1){
		igmph->MaxRespCode     = 0;
		igmph->GroupAddress[0] = 0;
		igmph->GroupAddress[1] = 0;
		igmph->GroupAddress[2] = 0;
		igmph->GroupAddress[3] = 0;
	}
	else{
		igmph->MaxRespCode     = maxResponseTime;
		igmph->GroupAddress[0] = groupAddress[0];
		igmph->GroupAddress[1] = groupAddress[1];
		igmph->GroupAddress[2] = groupAddress[2];
		igmph->GroupAddress[3] = groupAddress[3];
	}

	if(version!=3){
		igmph->ChkSum = ComputeCheckSum((unsigned short*)igmph, 8, NULL);
	}
	else{
		igmph->RSQ             = 0;
		igmph->QQIC            = 0;
		igmph->NumberOfSources = 0;
		igmph->ChkSum = ComputeCheckSum((unsigned short*)igmph, 12, NULL);
	}

	packSize = ttsize;
	
	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::BuildPacketGvrpSingle(BYTE eventIn, WORD vidIn)
{
	BYTE*           buff;
	int             size;
	LLCHeaderModPG  *llcmh;
	GARPHeaderPG    *garph;
	GARPAttributePG *garpa;

	packSize = 0;
	if(packet!=NULL){
		delete[] packet;
		packet = NULL;
	}

	if(eventIn>5) return false;

	if(eventIn==0) size = 10; // LLCHeaderModPG + GARPHeaderPG + GARPAttributePG (2 bytes) + 2 bytes for end mark
	else           size = 12; // LLCHeaderModPG + GARPHeaderPG + GARPAttributePG (4 bytes) + 2 bytes for end mark

	try{
		buff = new BYTE[size];
	}
	catch(...){
		buff = NULL;
	}
	if(buff==NULL) return false;

	llcmh = (LLCHeaderModPG*)&buff[0];
	llcmh->DSAPSSAP = 0x4242;
	llcmh->CTRL = 0x03;

	garph = (GARPHeaderPG*)&buff[3];
	garph->ProtocolID = htons(0x0001);
	garph->AttributeType = 0x01;

	garpa = (GARPAttributePG*)&buff[6];
	if(eventIn==0){
		garpa->Length = 0x02;
		garpa->Event  = eventIn;

		buff[8] = 0x00;	// first end mark
		buff[9] = 0x00;	// second end mark
	}
	else{
		garpa->Length = 0x04;
		garpa->Event  = eventIn;
		garpa->Value.w = htons(vidIn);

		buff[10] = 0x00;	// first end mark
		buff[11] = 0x00;	// second end mark
	}

	packet = buff;
	packSize = size;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::AddEthernetHeader(BYTE srcMACIn[6], BYTE dstMACIn[6], WORD lenTypeIn)
{
	EthernetHeaderPG* ehpg;
	BYTE* buff;
	int size;

	size = packSize + 14;

	try{
		buff = new BYTE[size];
	}
	catch(...){
		buff = NULL;
	}
	if(buff==NULL) return false;

	memcpy(&buff[0], &dstMACIn[0], 6);
	memcpy(&buff[6], &srcMACIn[0], 6);

	ehpg = (EthernetHeaderPG*)buff;
	ehpg->lenType = htons(lenTypeIn);

	memcpy(&buff[14], packet, packSize);

	delete[] packet;
	packet   = buff;
	packSize = size;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::AddEthernetHeaderGVRP(BYTE srcMACIn[6])
{
	BYTE dstMac[6];

	dstMac[0] = 0x01;
	dstMac[1] = 0x80;
	dstMac[2] = 0xC2;
	dstMac[3] = 0x00;
	dstMac[4] = 0x00;
	dstMac[5] = 0x21;

	return AddEthernetHeader(srcMACIn, dstMac, (WORD)packSize);
}

bool PacketBuilder::AddEthernetHeaderGMRP(BYTE srcMACIn[6])
{
	BYTE dstMac[6];

	dstMac[0] = 0x01;
	dstMac[1] = 0x80;
	dstMac[2] = 0xC2;
	dstMac[3] = 0x00;
	dstMac[4] = 0x00;
	dstMac[5] = 0x20;

	return AddEthernetHeader(srcMACIn, dstMac, (WORD)packSize);
}

bool PacketBuilder::AddEthernetHeaderSTP(BYTE srcMACIn[6])
{
	BYTE dstMac[6];

	dstMac[0] = 0x01;
	dstMac[1] = 0x80;
	dstMac[2] = 0xC2;
	dstMac[3] = 0x00;
	dstMac[4] = 0x00;
	dstMac[5] = 0x00;

	return AddEthernetHeader(srcMACIn, dstMac, (WORD)packSize);
}
//---------------------------------------------------------------------------

bool PacketBuilder::AddEthernetHeaderDotQ(BYTE srcMACIn[6], BYTE dstMACIn[6], WORD lenTypeIn, BYTE priorityIn, WORD vlanIdIn)
{
	EthernetHeaderDotQ* hdr;
	BYTE* buff;
	int size;
	WORD wVal;

	size = packSize + 18;

	try{
		buff = new BYTE[size];
	}
	catch(...){
		buff = NULL;
	}
	if(buff==NULL) return false;

	memcpy(&buff[0], &dstMACIn[0], 6);
	memcpy(&buff[6], &srcMACIn[0], 6);

	hdr = (EthernetHeaderDotQ*)buff;
	hdr->lenType = htons(lenTypeIn);

	hdr->tpid = htons(0x8100);	// 802.1Q / 802.1P identifier

	/*
	tci:
		- User priority: priorityIn (Ex.: GVRP = 7, ICMP = 4, Others = 0)
		- CFI: 0
		- VID: vlanIdIn
	*/
	wVal = (WORD)priorityIn;
	wVal = wVal << 13;
	wVal &= 0xE000;
	wVal += vlanIdIn & 0x0FFF;
	hdr->tci = htons(wVal);

	memcpy(&buff[18], packet, packSize);

	delete[] packet;
	packet   = buff;
	packSize = size;

	return true;
}

bool PacketBuilder::AddEthernetHeaderDotQDotQ(BYTE srcMACIn[6], BYTE dstMACIn[6], WORD lenTypeIn, BYTE priorityIn1, WORD vlanIdIn1, BYTE priorityIn2, WORD vlanIdIn2)
{
	EthernetHeaderDotQDotQ* hdr;
	BYTE* buff;
	int size;
	WORD wVal;

	size = packSize + 22;

	try{
		buff = new BYTE[size];
	}
	catch(...){
		buff = NULL;
	}
	if(buff==NULL) return false;

	memcpy(&buff[0], &dstMACIn[0], 6);
	memcpy(&buff[6], &srcMACIn[0], 6);

	hdr = (EthernetHeaderDotQDotQ*)buff;
	hdr->lenType = htons(lenTypeIn);

	hdr->tpid1 = htons(0x8100);	// 802.1Q / 802.1P identifier

	/*
	tci:
		- User priority: priorityIn (Ex.: GVRP = 7, ICMP = 4, Others = 0)
		- CFI: 0
		- VID: vlanIdIn
	*/
	wVal = (WORD)priorityIn1;
	wVal = wVal << 13;
	wVal &= 0xE000;
	wVal += vlanIdIn1 & 0x0FFF;
	hdr->tci1 = htons(wVal);

	hdr->tpid2 = htons(0x8100);	// 802.1Q / 802.1P identifier

	/*
	tci:
		- User priority: priorityIn (Ex.: GVRP = 7, ICMP = 4, Others = 0)
		- CFI: 0
		- VID: vlanIdIn
	*/
	wVal = (WORD)priorityIn2;
	wVal = wVal << 13;
	wVal &= 0xE000;
	wVal += vlanIdIn2 & 0x0FFF;
	hdr->tci2 = htons(wVal);

	memcpy(&buff[22], packet, packSize);

	delete[] packet;
	packet   = buff;
	packSize = size;

	return true;
}

//---------------------------------------------------------------------------

bool PacketBuilder::AddEthernetHeaderForIPMulticast(BYTE srcMACIn[6], DWORD dwDA, WORD lenTypeIn)
{
	BYTE dstMAC[6];
	in_addr ia;

	ia.S_un.S_addr = dwDA;

	dstMAC[0] = 0x01;
	dstMAC[1] = 0x00;
	dstMAC[2] = 0x5E;
	dstMAC[3] = ia.S_un.S_un_b.s_b2 & 0x7F;
	dstMAC[4] = ia.S_un.S_un_b.s_b3;
	dstMAC[5] = ia.S_un.S_un_b.s_b4;

	return AddEthernetHeader(srcMACIn, dstMAC, lenTypeIn);
}

bool PacketBuilder::IsIPv4MulticastAddress(DWORD addrIn)
{
	in_addr ia;

	ia.S_un.S_addr = addrIn;
	if(ia.S_un.S_un_b.s_b1<224) return false;
	if(ia.S_un.S_un_b.s_b1>239) return false;

	return true;
}

//---------------------------------------------------------------------------

BYTE* PacketBuilder::GetPacket(void)
{
	return packet;
}

DWORD PacketBuilder::GetPacketSize(void)
{
	return packSize;
}

//---------------------------------------------------------------------------

/*
bool PacketBuilder::SendPacket(DWORD qDestAddr, WORD qDestPort)
{
struct sockaddr_in dest;
int bwrote, xerr;
	if(!created) return false;
	if(sockRaw==INVALID_SOCKET) return false;
	if(packSize==0) return false;
	if(packet==NULL) return false;

	WSASetLastError(0);

	memset(&dest, 0, sizeof(dest));
	dest.sin_port=htons(qDestPort);
	dest.sin_family=AF_INET;
	dest.sin_addr.S_un.S_addr=qDestAddr;

	bwrote=sendto(sockRaw, (const char*)&packet, packSize, 0, (struct sockaddr*)&dest, sizeof(dest));
	if(bwrote==SOCKET_ERROR){
		xerr=WSAGetLastError();
		switch(xerr){
			case WSAETIMEDOUT :
				// The connection has been dropped, because of a network failure or because the system on the other end went down without notice.
				xerr=xerr;
				break;
			case WSAEHOSTUNREACH :
				// The remote host cannot be reached from this host at this time.
				xerr=xerr;
				break;
			case WSAEADDRNOTAVAIL :
				// The specified address is not available from the local machine.
				xerr=xerr;
				break;
			case WSAEINVAL :
				// An unknown flag was specified, or MSG_OOB was specified for a socket with SO_OOBINLINE enabled.
				xerr=xerr;
				break;
		}
		WSASetLastError(0);
		return false;
	}
	if(bwrote < packSize){
		// I have not written requested number of bytes
	}

	return true;
}
*/

//---------------------------------------------------------------------------
